#include "robot.h"

/****************************************************************/
/*                  Wander Thread Class                         */
/****************************************************************/

/****************************************************************/
/*  Function Name:  Wander                                      */
/*  Description:    Default Constructor                         */
/****************************************************************/
Wander::Wander()
{
    openDist = 50;
}

/****************************************************************/
/*  Function Name:  connectBot                                  */
/*  Description:    passes a pointer to a controller object     */
/*                  to allow for robot conrtrol                 */
/****************************************************************/
Wander::Wander(Controller *bot)
{
    com = bot;
    openDist = 50;
}

/****************************************************************/
/*  Function Name:  run                                         */
/*  Description:    main thread code, this is run when the start*/
/*                  fucntion is called                          */
/****************************************************************/
void Wander::run()
{
    bool drifted;
    char side;

    rightF = 0;
    leftF = 0;


    lock();
    while(rightF == 0 && leftF == 0)
    {
        rightF = com->getDistance('R', 'F');
        leftF = com->getDistance('L', 'F');
        //cout << leftF << ":" << rightF << endl;
    }
    unlock();
    stopped = false;


    com->move('F');

    while(stopped == false)
    {
        cout << "wander thread" << endl;
        this->setTerminationEnabled(false);
        wallCheck(drifted, side);
        lock();
        if(drifted == true)
        {
            std::cout << "drifted: " << side << std::endl;
            if(side == 'r')
            {
                com->spin('L');
                msleep(150);
            }
            else
            {
                com->spin('R');
                msleep(150);
            }
            drifted = false;
        }
        com->move('F');
        //        msleep(200);
        //        com->move('S');
        unlock();
        this->setTerminationEnabled(true);
        msleep(400);
    }

    std::cout << "Stopped" << std::endl;

}

/****************************************************************/
/*  Function Name:  run                                         */
/*  Description:    checks to see if the robot has moved closer */
/*                  to the wall and determines if a course      */
/*                  correction needs to be made                 */
/****************************************************************/
void Wander::wallCheck(bool &drifted, char &side)
{
    int newLeft, newRight, nextLeft, nextRight, rightDif, leftDif, centerDif;
    //lock and get new distance values
    lock();
    newLeft = com->getDistance('L', 'F');
    newRight = com->getDistance('R', 'F');
    unlock();

    //std::cout << "Right: " << rightF << "-" << newRight << std::endl;
    //std::cout << "Left: " << leftF << "-" << newLeft << std::endl;

    //get difrerences between old and new distances
    rightDif = rightF - newRight;
    leftDif = leftF - newLeft;

    lock();
    com->move('S');
    unlock();
    //only if within 50 cm of a wall
    if(newRight < openDist || newLeft < openDist)
    {
        centerDif = leftF - rightF;//difference between leftF and right values
        //if drift distance is greater than 1 cm
        if(abs(rightDif) > 1 || abs(leftDif) > 1)
        {
            //get new distance mesurements
            lock();
            nextRight = com->getDistance('R', 'F');
            nextLeft = com->getDistance('L', 'F');
            unlock();

            std::cout << "Dif Right" << rightF << "-" << nextRight << std::endl;
            std::cout << "Dif Left" << leftF << "-" << nextLeft << std::endl;

            //get new diffs
            rightDif = rightF - nextRight;
            leftDif = leftF - nextLeft;
            //if still seen to have drifted
            if(abs(rightDif) > 1 || abs(leftDif) > 1)
            {
                //if drifted to far and are getting bad data
                if((rightDif < -6 || leftDif < -6) && (nextRight < openDist && nextLeft < openDist))
                {
                    cout << "Bad Data" << endl;
                    drifted = true;
                    if(centerDif > 0)
                    {
                        side = 'l';
                    }
                    else
                    {
                        side = 'r';
                    }
                }
                else
                {
                    //com->move('S');
                    cout << "Drifted not bad data" << endl;
                    if(rightDif > 0 && side != 'l' && nextRight < openDist)
                    {
                        side = 'r';
                        drifted = true;
                        std::cout << "drifted: " << side << std::endl;
                    }
                    if(leftDif < 0 && side != 'l' && nextLeft < openDist)
                    {
                        side = 'r';
                        drifted = true;
                        std::cout << "drifted: " << side << std::endl;
                    }
                    if(rightDif < 0 && side != 'r' && nextRight < openDist)
                    {
                        side = 'l';
                        drifted = true;
                        std::cout << "drifted: " << side << std::endl;
                    }
                    if(leftDif > 0 && side != 'r' && nextLeft < openDist)
                    {
                        side = 'l';
                        drifted = true;
                        std::cout << "drifted: " << side << std::endl;
                    }

                    rightF = nextRight;
                    leftF = nextLeft;
                    //com->move('F');
                }
            }
        }
        //if leftF and right distances > 3
        else if(abs(centerDif) > 3  && (leftF < openDist && rightF < openDist))
        {
            std::cout << "Center: " << centerDif << std::endl;
            drifted = true;
            //closer to the leftF
            if(centerDif > 0)
            {
                side = 'r';
            }
            //closer to the right
            else
            {
                side = 'l';
            }
        }
        //didn't drift
        else
        {
            drifted = false;
        }
    }
    //didn't drift
    else
    {
        drifted = false;
    }
}


/****************************************************************/
/*  Function Name:  lock                                        */
/*  Description:    lock the mutex and make unable to terminate */
/****************************************************************/
void Wander::lock()
{
    mutex.lock();
    this->setTerminationEnabled(false);
}


/****************************************************************/
/*  Function Name:  unlock                                      */
/*  Description:   unlock the mutex and make unable to terminate*/
/****************************************************************/
void Wander::unlock()
{
    mutex.unlock();
    this->setTerminationEnabled(true);
}


/****************************************************************/
/*                  Main Robot Thread Class                     */
/****************************************************************/


/****************************************************************/
/*  Function Name:  Robot                                       */
/*  Description:    Default Constructor                         */
/****************************************************************/
Robot::Robot()
{
    openDist = 50;
}

/****************************************************************/
/*  Function Name:  connectBot                                  */
/*  Description:    passes in a pointer to a controller object  */
/*                  to allow control of the robot and passes    */
/*                  controller to wander thread                 */
/****************************************************************/
void Robot::connectBot(Controller *bot)
{
    com = bot;
    //wander.connectBot(com);
}

/****************************************************************/
/*  Function Name:  run                                         */
/*  Description:    main thread code, this is run when the start*/
/*                  fucntion is called                          */
/****************************************************************/
void Robot::run()
{
    bool opening = false;
    char side = ' ';
    int rightF, leftF, front, nextFront;
    wander = new Wander(com);

    if(!cam.connectCam(0))
    {
        return;
    }

    if(stopped == true)
    {
        stopped = false;
    }

    lock();
    rightF = com->getDistance('R', 'F');
    leftF = com->getDistance('L', 'F');
    front = com->getDistance('F', 'F');
    unlock();
    maze.start(front, front, -1, rightF, leftF);

    wandering = true;
    wander->start();
    distHeur(opening, side);
    cout << "opening: " << opening << endl;
    while(opening == true)
    {
        distHeur(opening, side);
        cout << "still opening: " << opening << endl;
        msleep(400);
    }

    while(stopped == false)
    {
        cout << "main thread" << endl;
        //this->setTerminationEnabled(false);
        if(stopped == true)
        {
            stopped = false;
            lock();
            rightF = com->getDistance('R', 'F');
            leftF = com->getDistance('L', 'F');
            unlock();
        }
        distHeur(opening, side);
        side = (side == 'B')? 'R' : side;
        //cout << "dist heur: " << "(" << opening << ":" << side << ")" << endl;

        if(opening == true)
        {
            lock();
            front = com->getDistance('F', 'F');
            nextFront = com->getDistance('F', 'F');
            unlock();

            while(front < (nextFront + 20))
            {
                lock();
                std::cout << "waiting to turn " << front << ", " << nextFront << std::endl;
                nextFront = com->getDistance('F', 'F');
                unlock();
                msleep(300);
            }
            std::cout << "waiting to turn " << front << ", " << nextFront << std::endl;

            killChildren();

            distHeur(opening, side);
            if(opening == true)
            {
                lock();
                rightF = com->getDistance('R', 'F');
                leftF = com->getDistance('L', 'F');
                front = com->getDistance('F', 'F');
                unlock();
                maze.addNode(front, front, -1, rightF, leftF);
                turn(side);
                msleep(400);
                ////////////////////////////////////////////////////////////
                //wandering = true; //only for debugging to ensure robot stops
                ////////////////////////////////////////////////////////////
                if(isRoom() == true)
                {
                    cout << "room" << endl;
                    maze.setRoom(side);
                    if(candle())
                    {
                        extinguish(side);
                    }
                    else
                    {
                        turn(oppDir(side));
                    }
                }

                turn(oppDir(side));
                //add a node for the intersection
                //cout << "Regular Intersection" << endl;
                turnTo(maze.checkNext());


                //nextFront = com->getDistance('F', 'F');
                wandering = true;
                wander = new Wander(com);
                wander->start();
                while(opening == true)
                {
                    distHeur(opening, side);
                    msleep(400);
                }
                opening = false;
                //////////////////////////////////////////////////////////
                //stopped = true; //only for debugging to ensure robot stops
                //////////////////////////////////////////////////////////
            }
        }

        if(wandering == false)
        {
            wandering = true;
            wander = new Wander(com);
            wander->start();
        }
        msleep(400);
    }
    killChildren();
    opening = false;
}


/****************************************************************/
/*  Function Name:  distHeur                                    */
/*  Description:    uses messurements taken from the sonar to   */
/*                  determine information about the environemnt */
/****************************************************************/
void Robot::distHeur(bool &opening, char &side)
{
    int rightF = 0, leftF = 0;

    while(leftF == 0 && rightF == 0)
    {
        lock();
        rightF = com->getDistance('R', 'F');
        leftF = com->getDistance('L', 'F');
        unlock();
        msleep(25);
    }

    cout << "dist heur: " << leftF << ":" << rightF << endl;
    if(rightF > openDist && leftF > openDist)
    {
        opening = true;
        side = 'B';
    }
    else if(rightF > openDist)
    {
        opening = true;
        side = 'R';
    }
    else if(leftF > openDist)
    {
        opening = true;
        side = 'L';
    }
    else
    {
        opening = false;
    }

    //opening = true;
}


/****************************************************************/
/*  Function Name:  killChilden                                 */
/*  Description:    kill all children that are runnning         */
/****************************************************************/
volatile void Robot::killChildren()
{
    wandering = false;
    while(!wander->isFinished())
    {
        wander->terminate();
    }
    wander->wait();
    std::cout << "Wander stopped" << std::endl;
    //wander.stop();
    delete wander;
    lock();
    com->move('S');
    unlock();
}


/****************************************************************/
/*  Function Name:  intersection                                */
/*  Description:    turn to the left or right to move down a    */
/*                  hallway                                     */
/****************************************************************/
void Robot::intersection(char side)
{
    //add node the map
    lock();
    if(side == 'B')
    {
        com->spin('R');
        msleep(1000);
        com->move('F');
    }
    else
    {
        com->spin('L');
        msleep(1000);
        com->move('F');
    }
    unlock();
}


/****************************************************************/
/*  Function Name:  isRoom                                      */
/*  Description:    determine if the opening near you is a room */
/****************************************************************/
bool Robot::isRoom()
{
    //int front;
    //turn(side);
    //msleep(400);
    if(cam.findStripe() == true)
    {
        cout << "its a room" << endl;
        return true;
    }
    else
    {
        return false;
    }
}


/****************************************************************/
/*  Function Name:  oppDir                                      */
/*  Description:    get the opposite dirredtion of side         */
/****************************************************************/
char Robot::oppDir(char side)
{
    if(side == 'R' || side == 'B')
    {
        return 'L';
    }
    else
    {
        return 'R';
    }
}


/****************************************************************/
/*  Function Name:  turn                                        */
/*  Description:    turn the robot towards side                 */
/****************************************************************/
void Robot::turn(char side)
{
    int posCorner = 0;
    int posLowest = 32000;
    int count = 0, reference;
    bool done = false, corner = false;
    char oppSide = oppDir(side);

    cout << "Turning: " << side << endl;
    lock();
    posCorner = com->getDistance(oppSide, 'F') + 5;
    //left = com->getDistance('L', 'F');
    //com->spin(side);
    //for(int i = 0; i < 40; i++)
    //unlock();
    while(done == false)
    {
        //lock();
        com->spin(side);
        //unlock();
        //msleep(25);
        //lock();
        //com->move('S');
        //right = com->getDistance('R', 'F');
        //left = com->getDistance('L', 'F');
        reference = com->getDistance(oppSide, 'F');
        //cout << "Turning: " << left << " : " << right << " Corner: " << posCorner << " Lowest: " << posLowest << " Count: " << count << endl;
        cout << "Turning: " << side << " Oppside: " << oppSide << " Ref: " << reference << " Corner: " << posCorner << " posLowest: " << posLowest << endl;
        if(reference < posLowest && corner == true)
        {
            posLowest = reference;
        }
        else if(reference > (posLowest) && corner == true)
        {
            count++;
        }
        else
        {
            done = false;
        }

        if(reference > posCorner)
        {
            corner = true;
            posCorner = reference;
            posLowest = 32000;
            done = false;
            count = 0;
        }

        if(count == 5)
        {
            done = true;
        }
    }
    do
    {
        com->spin(oppSide);
        //unlock();
        //msleep(25);
        //lock();
        //com->move('S');
        //right = com->getDistance('R', 'F');
        reference = com->getDistance(oppSide, 'F');
        cout << "Turning back: " << reference << " : " << posLowest << endl;
    }while(reference > (posLowest + 1));
    com->move('S');
    unlock();
}


/****************************************************************/
/*  Function Name:  candel                                      */
/*  Description:    determine if there is a candle in a room    */
/****************************************************************/
bool Robot::candle()
{
    bool uv = false;

    if(com->getUV() > 0)
    {
        uv = true;
    }

    return uv;
}


/****************************************************************/
/*  Function Name:  lock                                        */
/*  Description:    lock the mutex and make unable to terminate */
/****************************************************************/
void Robot::lock()
{

    mutex.lock();
    this->setTerminationEnabled(false);
}


/****************************************************************/
/*  Function Name:  unlock                                      */
/*  Description:   unlock the mutex and make unable to terminate*/
/****************************************************************/
void Robot::unlock()
{
    mutex.unlock();
    this->setTerminationEnabled(true);
}

void Robot::room()
{

}


/****************************************************************/
/*  Function Name:  extinguish                                  */
/*  Description:    move into the room, find the candle,        */
/*                  approach the candle and extinguish it       */
/****************************************************************/
void Robot::extinguish(char side)
{
    int front, nextFront, uv = 0, nextUV;
    bool candleFound = false;
    turn(side);
    lock();
    front = com->getDistance('F', 'F');
    nextFront = front;
    while(front < nextFront + 35)
    {
        wandering = true;
        wander = new Wander(com);
        wander->start();
        msleep(400);
        nextFront = com->getDistance('F', 'F');
    }
    com->spin('R');
    while(candleFound == false)
    {
        nextUV = com->getUV();
        if(nextUV > uv)
        {
            uv = nextUV;
        }
        else if(uv > nextUV)
        {
            candleFound = true;
        }
    }
    com->spin('L');
    while(com->getUV() != uv);
    com->move('S');
    unlock();
}


/****************************************************************/
/*  Function Name:  turnTo                                      */
/*  Description:    turn to a direction given by the map        */
/****************************************************************/
void Robot::turnTo(char dir)
{
    int front;

    if(dir == 'L')
    {
        turn('L');
        front = com->getDistance('F', 'F');
        maze.turn('L', front);
    }
    else if(dir == 'R')
    {
        turn('R');
        front = com->getDistance('F', 'F');
        maze.turn('R', front);
    }
    else if(dir == 'B')
    {
        turn('R');
        front = com->getDistance('F', 'F');
        maze.turn('R', front);
        turn('R');
        front = com->getDistance('F', 'F');
        maze.turn('R', front);
    }
}
